#ifndef _CONFIG_H_
#define _CONFIG_H_
//电机引脚
#define PWMD 12    //D电机转速
#define DIRD1 34 
#define DIRD2 35  //D电机方向
#define PWMC 8    //C电机转速
#define DIRC1 37 
#define DIRC2 36  //C电机方向
#define PWMB 9   //B电机转速
#define DIRB1 43 
#define DIRB2 42  //B电机方向
#define PWMA 5    //A电机转速
#define DIRA1 A4   //26  
#define DIRA2 A5   //27  //A电机方向

//in our configuration
//   (A)//   x+     \\(B)
//   
//   y+
//
//   (C)\\          //(D)

#define CAR_MOVE_VX_FORWARD_SPEED_QUICKLY -0.15
#define CAR_MOVE_VX_FORWARD_SPEED_SLOWLY  -0.1
#define CAR_MOVE_VX_BACK_SPEED_QUICKLY     0.15
#define CAR_MOVE_VX_BACK_SPEED_SLOWLY      0.15
#define CAR_MOVE_VY_RIGHT_TRANSLATION_SPEED_QUICKLY   -0.15
#define CAR_MOVE_VY_RIGHT_TRANSLATION_SPEED_SLOWLY    -0.1
#define CAR_MOVE_VY_LEFT_TRANSLATION_SPEED_QUICKLY     0.15
#define CAR_MOVE_VY_LEFT_TRANSLATION_SPEED_SLOWLY      0.1

#define CAR_MOVE_VW_LEFT_TURN_SPEED_QUICKLY 0.6
#define CAR_MOVE_VW_LEFT_TURN_SPEED_SLOWLY 0.25
#define CAR_MOVE_VW_RIGHT_TURN_SPEED_QUICKLY -0.6
#define CAR_MOVE_VW_RIGHT_TURN_SPEED_SLOWLY -0.25
#define CAR_MOVE_STOP_SPEED 0

#define CLAW_PIN 13
#define CLAW_INIT 20
#define CLAW_OPEN 20
#define CLAW_CLOSE 100

#define SERVO_HEAD 46
#define SERVO_UP 32
#define SERVO_DOWN 50

// float xn=0.08725, yn=0.105, r=0.03, K=abs(xn)+abs(yn); //m
float xn=0.0925, yn=0.1075, r=0.0475, K=abs(xn)+abs(yn); //m
int max_rmp= 330; //default: 330;
// float resolution=2*PI*r/1440.0;
float resolution=2*PI*r/1560.0;
float body_width = 0.215;
float body_length = 0.185;
float turn_contant = (body_length * body_length + body_width * body_width) / body_width / 2.0 * 0.98;
float pwma=0,pwmb=0,pwmc=0,pwmd=0;
float target_vel_lf = 0.0;
float target_vel_rf = 0.0;
float target_vel_lb = 0.0;
float target_vel_rb = 0.0;


//新添加的部分
float KP = 10.0;
float KI = 0.05;
float KD = 25.05;
float wA,wB,wC,wD;
float set_target_vx = 0.0;
float set_target_vy = 0.0;
float set_target_vw = 0.0;
unsigned int car_move_status = 0;
unsigned long calculate_time_period = 0;
unsigned long change_motor_action_time = 0;


bool car_move_vy_direction = false;
bool car_move_vx_direction = true;
unsigned long lastctrl;
double x=0,y=0,theta=0;

bool search_for_ballon_flag = true;
bool find_once_ballon_flag = false;
bool search_for_field_flag = false;
bool find_once_field_flag = false;

#define constrain(amt,low,high) \
  ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))



#endif
